#include "robot.h"
#include <iostream>
using namespace std;
//! Constructor
Robot::Robot(){
	double x=0., z=0.;
	double segLength = .01;
	double segHeight = .05;
	double segDistance = .025;
	double segEndHeight = segHeight/5;
	double segEndLength = segDistance*3;
	double jointHeight = segHeight/2;
	double jointLength = segEndLength;
	double jointRadius = segEndLength/2;
	double fingerLength = segLength*2;
	double fingerHeight = segHeight;
	double rot;

	// Auxiliar to calculate the height
	double atualHeight = .1;
	// Auxiliar to calculate the rotation
	double atualRotX, atualRotY;

	base = new piece;
	wheels = new piece;
	baseArm = new piece;
	secondArm = new piece;
	thirdArm = new piece;
	fourthArm = new piece;
	baseJoint = new piece;
	secondJoint = new piece;
	thirdJoint = new piece;
	fourthJoint = new piece;
	fingerJoint1 = new piece;
	fingerJoint2 = new piece;
	fingerJoint3 = new piece;
	fingerJoint4 = new piece;
	finger1Segment1 = new piece;
	finger2Segment1 = new piece;
	finger3Segment1 = new piece;
	finger4Segment1 = new piece;
	finger1Segment2 = new piece;
	finger2Segment2 = new piece;
	finger3Segment2 = new piece;
	finger4Segment2 = new piece;
	fingerTip1 = new piece;
	fingerTip2 = new piece;
	fingerTip3 = new piece;
	fingerTip4 = new piece;

	//! The 4 robot's wheels
	wheels->content = new Shape*[4];

	wheels->content[0] = new Sphere(vec3( x-.075, .05, z+.075), .05);
	wheels->content[1] = new Sphere(vec3( x+.075, .05, z+.075), .05);
	wheels->content[2] = new Sphere(vec3( x+.075, .05, z-.075), .05);
	wheels->content[3] = new Sphere(vec3( x-.075, .05, z-.075), .05);

	wheels->next = new piece*[1];
	wheels->next[0] = base;

	wheels->contentSize = 4;
	wheels->nextSize = 1;
	wheels->tipo = 2;

	//! The robot's base
	base->content = new Shape*[1];
	base->content[0] = new Parallelepiped(vec3( x, .1, z ), .1, .2, .2);
	
	base->next = new piece*[1];
	base->next[0] = baseArm;

	base->contentSize = 1;
	base->nextSize = 1;
	base->tipo = 1;

	//! The robot's baseArm
	baseArm->content = new Shape*[17];	

	for (int i=0; i<4; i++){
		baseArm->content[i*4] = new Parallelepiped( vec3( x-segDistance, atualHeight+segHeight/2, z+segDistance ), segHeight, segLength, segLength );
		baseArm->content[i*4+1] = new Parallelepiped( vec3( x+segDistance, atualHeight+segHeight/2, z+segDistance ), segHeight, segLength, segLength );
		baseArm->content[i*4+2] = new Parallelepiped( vec3( x+segDistance, atualHeight+segHeight/2, z-segDistance ), segHeight, segLength, segLength );
		baseArm->content[i*4+3] = new Parallelepiped( vec3( x-segDistance, atualHeight+segHeight/2, z-segDistance ), segHeight, segLength, segLength );
	
		atualHeight = atualHeight + segHeight;
	}

	baseArm->content[16] = new Parallelepiped( vec3( x, atualHeight + segEndHeight/2 , z ), segEndHeight, segEndLength, segEndLength );

	baseArm->next = new piece*[1];
	baseArm->next[0] = baseJoint;

	baseArm->contentSize = 17;
	baseArm->nextSize = 1;
	baseArm->tipo = 1;

	atualHeight = atualHeight + segEndHeight;

	//! BaseJoint
	baseJoint->content = new Shape*[1];
	baseJoint->content[0] = new Cylinder( vec3( x, atualHeight + jointHeight/2 , z ), jointLength/2, jointHeight );

	baseJoint->next = new piece*[1];
	baseJoint->next[0] = secondArm;

	baseJoint->contentSize = 1;
	baseJoint->nextSize = 1;
	baseJoint->tipo = 6;

	atualHeight = atualHeight + jointHeight;

	//! The robot's secondArm
	secondArm->content = new Shape*[18];

	secondArm->content[0] = new Parallelepiped( vec3( x, atualHeight + segEndHeight/2 , z ), segEndHeight, segEndLength, segEndLength );

	atualHeight = atualHeight + segEndHeight;
	
	for (int i=0; i<4; i++){
		secondArm->content[i*4+1] = new Parallelepiped( vec3( x-segDistance, atualHeight+segHeight/2, z+segDistance ), segHeight, segLength, segLength );
		secondArm->content[i*4+2] = new Parallelepiped( vec3( x+segDistance, atualHeight+segHeight/2, z+segDistance ), segHeight, segLength, segLength );
		secondArm->content[i*4+3] = new Parallelepiped( vec3( x+segDistance, atualHeight+segHeight/2, z-segDistance ), segHeight, segLength, segLength );
		secondArm->content[i*4+4] = new Parallelepiped( vec3( x-segDistance, atualHeight+segHeight/2, z-segDistance ), segHeight, segLength, segLength );
	
		atualHeight = atualHeight + segHeight;
	}

	secondArm->content[17] = new Parallelepiped( vec3( x, atualHeight + segEndHeight/2 , z ), segEndHeight, segEndLength, segEndLength );

	secondArm->next = new piece*[1];
	secondArm->next[0] = secondJoint;

	secondArm->contentSize = 18;
	secondArm->nextSize = 1;
	secondArm->tipo = 1;

	atualHeight = atualHeight + segEndHeight;

	//! secondJoint
	secondJoint->content = new Shape*[1];

	secondJoint->content[0] = new Cylinder( vec3( x, atualHeight + jointLength/2 , z ), jointLength/2, jointHeight*2 );
	secondJoint->content[0]->rotX( -90.*DegreesToRadians, x, atualHeight + jointLength/2 , z  );

	secondJoint->next = new piece*[1];
	secondJoint->next[0] = thirdArm;

	secondJoint->contentSize = 1;
	secondJoint->nextSize = 1;
	secondJoint->tipo = 6;

	atualHeight = atualHeight + jointLength;

	//! thirdArm
	thirdArm->content = new Shape*[18];

	thirdArm->content[0] = new Parallelepiped( vec3( x, atualHeight + segEndHeight/2 , z ), segEndHeight, segEndLength, segEndLength );

	atualHeight = atualHeight + segEndHeight;
	

	for (int i=0; i<4; i++){
		thirdArm->content[i*4+1] = new Parallelepiped( vec3( x-segDistance, atualHeight+segHeight/2, z+segDistance ), segHeight, segLength, segLength );
		thirdArm->content[i*4+2] = new Parallelepiped( vec3( x+segDistance, atualHeight+segHeight/2, z+segDistance ), segHeight, segLength, segLength );
		thirdArm->content[i*4+3] = new Parallelepiped( vec3( x+segDistance, atualHeight+segHeight/2, z-segDistance ), segHeight, segLength, segLength );
		thirdArm->content[i*4+4] = new Parallelepiped( vec3( x-segDistance, atualHeight+segHeight/2, z-segDistance ), segHeight, segLength, segLength );
	
		atualHeight = atualHeight + segHeight;
	}

	thirdArm->content[17] = new Parallelepiped( vec3( x, atualHeight + segEndHeight/2 , z ), segEndHeight, segEndLength, segEndLength );

	thirdArm->next = new piece*[1];
	thirdArm->next[0] = thirdJoint;

	thirdArm->contentSize = 18;
	thirdArm->nextSize = 1;
	thirdArm->tipo = 1;

	atualHeight = atualHeight + segEndHeight;

	// Rotation
		atualRotY = atualHeight + segEndLength/2;
		rot = 90;

	//! Third joint
	thirdJoint->content = new Shape*[1];

	thirdJoint->content[0] = new Cylinder( vec3( x, atualHeight + segEndLength/2 , z ), jointLength/2, segEndLength );

	thirdJoint->next = new piece*[1];
	thirdJoint->next[0] = fourthArm;

	thirdJoint->contentSize = 1;
	thirdJoint->nextSize = 1;
	thirdJoint->tipo = 6;

	atualHeight = atualHeight + segEndLength;

	//! fouth arm
	fourthArm->content = new Shape*[18];

	fourthArm->content[0] = new Parallelepiped( vec3( x, atualHeight + segEndHeight/2 , z ), segEndHeight, segEndLength, segEndLength );

	atualHeight = atualHeight + segEndHeight;

	for (int i=0; i<4; i++){
		fourthArm->content[i*4+1] = new Parallelepiped( vec3( x-segDistance, atualHeight+segHeight/2, z+segDistance ), segHeight, segLength, segLength );
		fourthArm->content[i*4+2] = new Parallelepiped( vec3( x+segDistance, atualHeight+segHeight/2, z+segDistance ), segHeight, segLength, segLength );
		fourthArm->content[i*4+3] = new Parallelepiped( vec3( x+segDistance, atualHeight+segHeight/2, z-segDistance ), segHeight, segLength, segLength );
		fourthArm->content[i*4+4] = new Parallelepiped( vec3( x-segDistance, atualHeight+segHeight/2, z-segDistance ), segHeight, segLength, segLength );

		atualHeight = atualHeight + segHeight;
	}

	fourthArm->content[17] = new Parallelepiped( vec3( x, atualHeight + segEndHeight/2 , z ), segEndHeight, segEndLength, segEndLength );

	fourthArm->next = new piece*[1];
	fourthArm->next[0] = fourthJoint;

	fourthArm->contentSize = 18;
	fourthArm->nextSize = 1;
	fourthArm->tipo = 1;

	atualHeight = atualHeight + segEndHeight;

	//! The robot's fourth joint
	fourthJoint->content = new Shape*[1];

	fourthJoint->content[0] = new Sphere( vec3( x, atualHeight + jointRadius , z ), jointRadius );
	
	fourthJoint->next = new piece*[4];
	fourthJoint->next[0] = finger1Segment1;
	fourthJoint->next[1] = finger2Segment1;
	fourthJoint->next[2] = finger3Segment1;
	fourthJoint->next[3] = finger4Segment1;

	fourthJoint->contentSize = 1;
	fourthJoint->nextSize = 4;
	fourthJoint->tipo = 2;

	atualHeight = atualHeight + jointRadius;

	//! The robot's fingers
	finger1Segment1->content = new Shape*[4];
	finger2Segment1->content = new Shape*[4];
	finger3Segment1->content = new Shape*[4];
	finger4Segment1->content = new Shape*[4];

	for (int i=0; i<4; i++){
		finger1Segment1->content[i] = new Parallelepiped( vec3( x, atualHeight+fingerHeight/2, z ), fingerHeight, fingerLength, fingerLength );
		finger2Segment1->content[i] = new Parallelepiped( vec3( x, atualHeight+fingerHeight/2, z ), fingerHeight, fingerLength, fingerLength );
		finger3Segment1->content[i] = new Parallelepiped( vec3( x, atualHeight+fingerHeight/2, z ), fingerHeight, fingerLength, fingerLength );
		finger4Segment1->content[i] = new Parallelepiped( vec3( x, atualHeight+fingerHeight/2, z ), fingerHeight, fingerLength, fingerLength );

		atualHeight = atualHeight + fingerHeight;
	}

	finger1Segment1->next = new piece*[1];
	finger2Segment1->next = new piece*[1];
	finger3Segment1->next = new piece*[1];
	finger4Segment1->next = new piece*[1];

	finger1Segment1->next[0] = fingerJoint1;
	finger2Segment1->next[0] = fingerJoint2;
	finger3Segment1->next[0] = fingerJoint3;
	finger4Segment1->next[0] = fingerJoint4;

	finger1Segment1->contentSize = 4;
	finger1Segment1->nextSize = 1;
	finger1Segment1->tipo = 1;
	finger2Segment1->contentSize = 4;
	finger2Segment1->nextSize = 1;
	finger2Segment1->tipo = 1;
	finger3Segment1->contentSize = 4;
	finger3Segment1->nextSize = 1;
	finger3Segment1->tipo = 1;
	finger4Segment1->contentSize = 4;
	finger4Segment1->nextSize = 1;
	finger4Segment1->tipo = 1;

	//! The robot's finger joint
	fingerJoint1->content = new Shape*[1];
	fingerJoint2->content = new Shape*[1];
	fingerJoint3->content = new Shape*[1];
	fingerJoint4->content = new Shape*[1];
	
	fingerJoint1->content[0] = new Sphere( vec3( x, atualHeight + jointRadius/4 , z ), jointRadius/2 );
	fingerJoint2->content[0] = new Sphere( vec3( x, atualHeight + jointRadius/4 , z ), jointRadius/2 );
	fingerJoint3->content[0] = new Sphere( vec3( x, atualHeight + jointRadius/4 , z ), jointRadius/2 );
	fingerJoint4->content[0] = new Sphere( vec3( x, atualHeight + jointRadius/4 , z ), jointRadius/2 );

	fingerJoint1->next = new piece*[1];
	fingerJoint2->next = new piece*[1];
	fingerJoint3->next = new piece*[1];
	fingerJoint4->next = new piece*[1];

	fingerJoint1->next[0] = finger1Segment2;
	fingerJoint2->next[0] = finger2Segment2;
	fingerJoint3->next[0] = finger3Segment2;
	fingerJoint4->next[0] = finger4Segment2;

	fingerJoint1->contentSize = 1;
	fingerJoint1->nextSize = 1;
	fingerJoint1->tipo = 2;
	fingerJoint2->contentSize = 1;
	fingerJoint2->nextSize = 1;
	fingerJoint2->tipo = 2;
	fingerJoint3->contentSize = 1;
	fingerJoint3->nextSize = 1;
	fingerJoint3->tipo = 2;
	fingerJoint4->contentSize = 1;
	fingerJoint4->nextSize = 1;
	fingerJoint4->tipo = 2;

	//! The robot's fingers
	finger1Segment2->content = new Shape*[4];
	finger2Segment2->content = new Shape*[4];
	finger3Segment2->content = new Shape*[4];
	finger4Segment2->content = new Shape*[4];

	for (int i=0; i<4; i++){
		finger1Segment2->content[i] = new Parallelepiped( vec3( x, atualHeight+fingerHeight/2, z ), fingerHeight, fingerLength, fingerLength );
		finger2Segment2->content[i] = new Parallelepiped( vec3( x, atualHeight+fingerHeight/2, z ), fingerHeight, fingerLength, fingerLength );
		finger3Segment2->content[i] = new Parallelepiped( vec3( x, atualHeight+fingerHeight/2, z ), fingerHeight, fingerLength, fingerLength );
		finger4Segment2->content[i] = new Parallelepiped( vec3( x, atualHeight+fingerHeight/2, z ), fingerHeight, fingerLength, fingerLength );

		atualHeight = atualHeight + fingerHeight;
	}

	finger1Segment2->next = new piece*[1];
	finger2Segment2->next = new piece*[1];
	finger3Segment2->next = new piece*[1];
	finger4Segment2->next = new piece*[1];

	finger1Segment2->next[0] = fingerTip1;
	finger2Segment2->next[0] = fingerTip2;
	finger3Segment2->next[0] = fingerTip3;
	finger4Segment2->next[0] = fingerTip4;

	finger1Segment2->contentSize = 4;
	finger1Segment2->nextSize = 1;
	finger1Segment2->tipo = 1;
	finger2Segment2->contentSize = 4;
	finger2Segment2->nextSize = 1;
	finger2Segment2->tipo = 1;
	finger3Segment2->contentSize = 4;
	finger3Segment2->nextSize = 1;
	finger3Segment2->tipo = 1;
	finger4Segment2->contentSize = 4;
	finger4Segment2->nextSize = 1;
	finger4Segment2->tipo = 1;

	//! The robot's fingers tips
	fingerTip1->content = new Shape*[1];
	fingerTip2->content = new Shape*[1];
	fingerTip3->content = new Shape*[1];
	fingerTip4->content = new Shape*[1];

	fingerTip1->content[0] = new Pyramid( vec3( x, atualHeight + jointHeight/2 , z ), jointHeight, jointHeight );
	fingerTip2->content[0] = new Pyramid( vec3( x, atualHeight + jointHeight/2 , z ), jointHeight, jointHeight );
	fingerTip3->content[0] = new Pyramid( vec3( x, atualHeight + jointHeight/2 , z ), jointHeight, jointHeight );
	fingerTip4->content[0] = new Pyramid( vec3( x, atualHeight + jointHeight/2 , z ), jointHeight, jointHeight );

	fingerTip1->next = NULL;
	fingerTip2->next = NULL;
	fingerTip3->next = NULL;
	fingerTip4->next = NULL;

	fingerTip1->contentSize = 1;
	fingerTip1->nextSize = 0;
	fingerTip1->tipo = 3;
	fingerTip2->contentSize = 1;
	fingerTip2->nextSize = 0;
	fingerTip2->tipo = 3;
	fingerTip3->contentSize = 1;
	fingerTip3->nextSize = 0;
	fingerTip3->tipo = 3;
	fingerTip4->contentSize = 1;
	fingerTip4->nextSize = 0;
	fingerTip4->tipo = 3;

	// Set the initial inclination
	vec3 position = thirdJoint->content[0]->getPosition();
	rotateZ( thirdJoint->next[0], -90.*DegreesToRadians, position.x, position.y, position.z );
}

//! private method to translate the piece in the X direction
void Robot::moveX(piece * peca, double distance){
	vec3 original;

	for (int i=0; i<peca->contentSize; i++){
		original = peca->content[i]->getPosition();
		peca->content[i]->setPosition( vec3( original.x + distance, original.y, original.z) );
	}
	for (int i=0; i<peca->nextSize; i++){
		moveX(peca->next[i], distance);
	}	
}

//! private method to translate the piece in the Y direction
void Robot::moveY(piece * peca, double distance){
	vec3 original;

	for (int i=0; i<peca->contentSize; i++){
		original = peca->content[i]->getPosition();
		peca->content[i]->setPosition( vec3( original.x, original.y, original.z+ distance) );
	}
	for (int i=0; i<peca->nextSize; i++){
		moveY(peca->next[i], distance);
	}	
}

//! private method to rotate the piece in the X direction
void Robot::rotateX(piece * peca, double radians, double axisX, double axisY, double axisZ){
	for (int i=0; i<peca->contentSize; i++){
		peca->content[i]->rotX( radians, axisX, axisY, axisZ );
	}
	for (int i=0; i<peca->nextSize; i++){
		rotateX(peca->next[i], radians, axisX, axisY, axisZ);
	}	
}

//! private method to rotate the piece in the Y direction
void Robot::rotateY(piece * peca, double radians, double axisX, double axisY, double axisZ){
	for (int i=0; i<peca->contentSize; i++){
		peca->content[i]->rotY( radians, axisX, axisY, axisZ );
	}
	for (int i=0; i<peca->nextSize; i++){
		rotateY(peca->next[i], radians, axisX, axisY, axisZ);
	}	
}

//! private method to rotate the piece in the Z direction
void Robot::rotateZ(piece * peca, double radians, double axisX, double axisY, double axisZ){
	for (int i=0; i<peca->contentSize; i++){
		peca->content[i]->rotZ( radians, axisX, axisY, axisZ );
	}
	for (int i=0; i<peca->nextSize; i++){
		rotateZ(peca->next[i], radians, axisX, axisY, axisZ);
	}	
}

//! Move the robot left
void Robot::moveLeft(double distance){
	moveX( wheels, -distance );
}

//! Move the robot right
void Robot::moveRight(double distance){
	moveX( wheels, distance );
}

//! Move the robot to front
void Robot::moveFront(double distance){
	moveY( wheels, -distance );
}

//! Move the robot to back
void Robot::moveBack(double distance){
	moveY( wheels, -distance );
}

//! Use the baseJoint
void Robot::useBaseJoint(double radians){
	vec3 position = baseJoint->content[0]->getPosition();
	rotateY( baseJoint->next[0] , radians,  position.x , position.y, position.z);
}

//! Use the secondJoint
void Robot::useSecondJoint(double radians){
	vec3 position = secondJoint->content[0]->getPosition();
	rotateZ( secondJoint->next[0] , radians,  position.x , position.y, position.z);
}

//! Returns the private field base
Parallelepiped * * Robot::getBase(){
	return (Parallelepiped * *)base->content;
}

//! Returns the private field wheels
Sphere * * Robot::getWheels(){
	return (Sphere * *)wheels->content;
}

//! Returns the private field baseArm
Parallelepiped * * Robot::getBaseArm(){
	return (Parallelepiped * *)baseArm->content;
}

//! Returns the private field secondArm
Parallelepiped * * Robot::getSecondArm(){
	return (Parallelepiped * *)secondArm->content;
}

//! Returns the private field thirdArm
Parallelepiped * * Robot::getThirdArm(){
	return (Parallelepiped * *)thirdArm->content;
}

//! Returns the private field fourthArm
Parallelepiped * * Robot::getFourthArm(){
	return (Parallelepiped * *)fourthArm->content;
}

//! Returns the private field finger1Segment1
Parallelepiped * * Robot::getFinger1Segment1(){
	return (Parallelepiped * *)finger1Segment1->content;
}

//! Returns the private field finger2Segment1
Parallelepiped * * Robot::getFinger2Segment1(){
	return (Parallelepiped * *)finger1Segment1->content;
}

//! Returns the private field finger3Segment1
Parallelepiped * * Robot::getFinger3Segment1(){
	return (Parallelepiped * *)finger3Segment1->content;
}

//! Returns the private field finger4Segment1
Parallelepiped * * Robot::getFinger4Segment1(){
	return (Parallelepiped * *)finger4Segment1->content;
}

//! Returns the private field finger1Segment2
Parallelepiped * * Robot::getFinger1Segment2(){
	return (Parallelepiped * *)finger1Segment2->content;
}

//! Returns the private field finger2Segment2
Parallelepiped * * Robot::getFinger2Segment2(){
	return (Parallelepiped * *)finger1Segment2->content;
}

//! Returns the private field finger3Segment2
Parallelepiped * * Robot::getFinger3Segment2(){
	return (Parallelepiped * *)finger3Segment2->content;
}

//! Returns the private field finger4Segment2
Parallelepiped * * Robot::getFinger4Segment2(){
	return (Parallelepiped * *)finger4Segment2->content;
}

//! Returns the private field baseJoint
Cylinder * * Robot::getBaseJoint(){
	return (Cylinder * *)baseJoint->content;
}

//! Returns the private field secondJoint
Cylinder * * Robot::getSecondJoint(){
	return (Cylinder * *)secondJoint->content;
}

//! Returns the private field thirdJoint
Cylinder * * Robot::getThirdJoint(){
	return (Cylinder * *)thirdJoint->content;
}

//! Returns the private field fourthJoint
Sphere * * Robot::getFourthJoint(){
	return (Sphere * *)fourthJoint->content;
}

//! Returns the private field fingerJoint
Sphere * * Robot::getFingerJoint1(){
	return (Sphere * *)fingerJoint1->content;
}

//! Returns the private field fingerJoint
Sphere * * Robot::getFingerJoint2(){
	return (Sphere * *)fingerJoint2->content;
}

//! Returns the private field fingerJoint
Sphere * * Robot::getFingerJoint3(){
	return (Sphere * *)fingerJoint3->content;
}

//! Returns the private field fingerJoint
Sphere * * Robot::getFingerJoint4(){
	return (Sphere * *)fingerJoint4->content;
}

//! Returns the private field fingerTip
Pyramid * * Robot::getFingerTip1(){
	return (Pyramid * *)fingerTip1->content;
}

//! Returns the private field fingerTip
Pyramid * * Robot::getFingerTip2(){
	return (Pyramid * *)fingerTip2->content;
}

//! Returns the private field fingerTip
Pyramid * * Robot::getFingerTip3(){
	return (Pyramid * *)fingerTip3->content;
}

//! Returns the private field fingerTip
Pyramid * * Robot::getFingerTip4(){
	return (Pyramid * *)fingerTip4->content;
}

//! Returns the first piece
piece * Robot::getFirstPiece(){
	return wheels;
}
